#include "Plane.h"
#include <algorithm>

namespace Engine
{
	void Plane::Normalize()
	{
		float n = 1 / m_normal.Length();

		m_normal *= n;
		m_d *= n;
	}

	Plane::Plane(const Vec3& p0, const Vec3& p1, const Vec3& p2)
	{
		m_normal = CrossProduct((p2 - p0),(p1 - p0));
		m_normal.Normalize();

		m_d = DotProduct(p0, m_normal);

		Normalize();
	}

	void Plane::Set(const Vec3& p0, const Vec3& p1, const Vec3& p2)
	{
		m_normal = CrossProduct((p2 - p0),(p1 - p0));
		m_normal.Normalize();

		m_d = DotProduct(p0, m_normal);

		Normalize();
	}

	float Plane::GetDistanceToPoint(const Vec3& point, Plane::Pos* p) const
	{
		float dist = (m_d - DotProduct(point, m_normal));

		if(p)
			*p = (dist == 0.0f ? Plane::IN_PLANE : dist < 0.0f ? Plane::BEHIND : Plane::IN_FRONT);

		return dist;
	}

	float Plane::GetDistanceToLineSeg(const LineSeg& line, Plane::Pos* p) const
	{
		Plane::Pos a, b;
		float distA = GetDistanceToPoint(line.GetStart(), &a);
		float distB = GetDistanceToPoint(line.GetEnd(), &b);
		

		if((a == Plane::BEHIND) && (b == Plane::IN_FRONT) ||
		   (b == Plane::BEHIND) && (a == Plane::IN_FRONT))
		{
			if(p)
				*p = Plane::IN_PLANE;

			return 0.0f;
		}

		if(distA > 0.0f)
		{
			if(p) *p = Plane::IN_FRONT;
			return std::min(distA, distB);
		}

		if(p) *p = Plane::BEHIND;
		return std::max(distA, distB);
	}

	void Plane::SetABCD(float a, float b, float c, float d)
	{
		m_normal.x = a;
		m_normal.y = b;
		m_normal.z = c;
		m_d = d;

		Normalize();
	}

}
